#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <dirent.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <zed_depth/Object_center_depth.h>
#include <yolov5_ros/Object.h>
#include <cmath>
#include <cv_bridge/cv_bridge.h>
#include <string>

std::string g_name = "";
int g_center_x = 0;
int g_center_y = 0;
int g_object_w = 0;
int g_object_h = 0;
cv::Mat g_depth_image;
int g_image_width = 0;
int g_image_height = 0;

void depthCallback(const sensor_msgs::Image::ConstPtr& msg)
{
    //g_depth_image = (float*)(&msg->data[0]);
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    g_depth_image = cv_ptr->image;
    g_image_width = msg->width;
    g_image_height = msg->height;
}

void objectCallback(const yolov5_ros::Object& msg) {
    g_name = msg.name.c_str();
    g_center_x = std::round(msg.x);
    g_center_y = std::round(msg.y);
    g_object_w = std::round(msg.w);
    g_object_h = std::round(msg.h);
}

//blog.csdn.net/qq_29985391/article/details/80247724
int main(int argc, char** argv) {
    ros::init(argc, argv, "zed_depth");
    ros::NodeHandle n;
    ros::Subscriber zed_depth = n.subscribe("/zed/zed_node/depth/depth_registered", 1, depthCallback);
    ros::Subscriber object_detection = n.subscribe("/detection/object", 1, objectCallback);
    ros::Publisher object_depth = n.advertise<zed_depth::Object_center_depth>("/detection/object_center_depth", 1);
    ROS_INFO("Init done");

    int count = 0;
    while(1) {
        ros::spinOnce();
        if(g_name=="") {
            count++;
            continue;
        }
        ROS_INFO("%d times", count);
        zed_depth::Object_center_depth msg;
        msg.name = g_name;
        msg.x = g_center_x;
        msg.y = g_center_y;
        bool done = false;
        for(int j = g_center_x; (j < g_center_x + g_object_w/2) && (j < g_image_width); j++) {
            for(int i = g_center_y; (i < g_center_x + g_object_w/2) && (i < g_image_height); i++) {
                if(std::isnan(g_depth_image.at<float>(i, j)) || std::isinf(g_depth_image.at<float>(i, j)) || g_depth_image.at<float>(i, j) < (float)1e-1) {
                    continue;
                }
                else {
                    msg.depth = g_depth_image.at<float>(i, j);
                    done = true;
                }
                if(done) {
                    break;
                }
            }
            if(done) {
                break;
            }
        }
        if(done) {
            object_depth.publish(msg);
            ROS_INFO("width: %f", msg.depth);
        }
        else {
            ROS_INFO("Depth is nan");
        }

        // if(std::isnan(g_depth_image.at<float>(g_center_x, g_center_y))) {
        //     ROS_INFO("Depth is nan");
        // }
        // else {
        //     msg.depth = g_depth_image.at<float>(g_center_x, g_center_y);
        //     object_depth.publish(msg);
        //     ROS_INFO("width: %f", msg.depth);
        // }
        g_center_x = 0;
        g_center_y = 0;
        g_name = "";
        done = false;
        if(count == INT32_MAX) {
            break;
        }
        count++;
    }
    return 0;
}
